import cv2


def draw_trajectories(map_img, usvs, ports, assignments, out_path, scale):
    if len(map_img.shape) == 2 or map_img.shape[2] == 1:
        map_img = cv2.cvtColor(map_img, cv2.COLOR_GRAY2BGR)
    map_img = cv2.resize(map_img, None, fx=scale, fy=scale)

    # 画港口（红色）
    for port in ports.values():
        x, y = int(port[0] * scale), int(port[1] * scale)
        cv2.circle(map_img, (x, y), 4, (0, 0, 255), -1)  # 红色港口点

    # 画无人艇初始位置（绿色）
    for usv in usvs:
        x, y = usv['pos']
        x, y = int(x * scale), int(y * scale)
        cv2.circle(map_img, (x, y), 4, (0, 255, 0), -1)  # 绿色点代表无人艇
        # cv2.putText(map_img, str(usv['id']), (x + 4, y + 4), cv2.FONT_HERSHEY_SIMPLEX, 0.3, (0, 255, 0), 1)

    # 画轨迹（蓝色）
    for assign in assignments:
        traj = assign['trajectory']
        for i in range(len(traj) - 1):
            x1, y1 = int(traj[i][0] * scale), int(traj[i][1] * scale)
            x2, y2 = int(traj[i + 1][0] * scale), int(traj[i + 1][1] * scale)
            cv2.line(map_img, (x1, y1), (x2, y2), (255, 0, 0), 2)

    cv2.imwrite(out_path, map_img)
    print(f"轨迹图已保存至: {out_path}")
